import cv2
import numpy as np
import yaml
import socket
import time
from socket import timeout as TimeoutException
# from threading import Thread


def detect():
    # global message
    global connected
    cfg = yaml.load(open('/home/pi/wingo-color-tracker/config.yaml'), Loader=yaml.FullLoader)
    url, thres_low, thres_high, mp = cfg['url'], cfg['thres']['low'], cfg['thres']['high'], cfg['mp']

    # open camera
    cap = cv2.VideoCapture(url)
    # cap = cv2.VideoCapture(0)

    # read 1 frame
    _, frame = cap.read()

    w = int(cap.get(cv2.CAP_PROP_FRAME_WIDTH))
    h = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT))
    cx = int(w / 2)  # center x
    cy = int(h / 2)  # center y

    while 1:
        _, frame = cap.read()

        # mark center
        cv2.circle(img=frame, center=(cx, cy), radius=5, color=(0, 0, 0), thickness=-1)

        # apply gaussian filter
        blurred = cv2.GaussianBlur(frame, (11, 11), 0)

        # convert bgr to hsv
        hsv = cv2.cvtColor(blurred, cv2.COLOR_BGR2HSV)

        # low & high thresholds of green color in hsv
        thres_low = np.array(thres_low)
        thres_high = np.array(thres_high)

        # mask
        mask = cv2.inRange(hsv, thres_low, thres_high)

        # erosion
        mask = cv2.erode(mask, None, iterations=2)

        # dilation after erosion to reduce noise
        mask = cv2.dilate(mask, None, iterations=2)
        # cv2.imshow('mask', mask)

        # find all contours
        cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2]
        # if len(cnts) == 0:
        #     continue

        # if at least 1 contour exists
        if len(cnts) > 0:
            # find biggest shape
            c = max(cnts, key=cv2.contourArea)

            # draw the edge
            ((x, y), radius) = cv2.minEnclosingCircle(c)

            m = cv2.moments(c)

            # calculate the centroid of the shape
            obj_center = (int(m["m10"] / m["m00"]), int(m["m01"] / m["m00"]))

            # only deal with big enough circle
            if radius > 1:
                # mark the edge of circle
                cv2.circle(img=frame, center=(int(x), int(y)), radius=int(radius), color=(0, 255, 255), thickness=2)

                # mark the center of circle
                cv2.circle(img=frame, center=obj_center, radius=5, color=(0, 0, 255), thickness=-1)

                if cx - mp < obj_center[0] < cx + mp and cy - mp < obj_center[1] < cy + mp:
                    # text = 'Matched!'
                    # text_color = (0, 255, 0)
                    matched = '1'

                else:
                    # text = f'x={obj_center[0] - cx}, y={obj_center[1] - cy}'
                    # text_color = (0, 0, 255)
                    matched = '0'
                message = ' '.join([matched, str(cx - obj_center[0]), str(cy - obj_center[1])])
                send(host=cfg['host'], port=cfg['port'], response=message)
                # cv2.putText(img=frame, text=text, org=(0, h - 10), fontFace=cv2.FONT_HERSHEY_SIMPLEX, fontScale=1,
                #             color=text_color, thickness=2, lineType=cv2.LINE_AA)

        # cv2.imshow('frame', frame)

        # press 'q' to stop
        if cv2.waitKey(1) == ord('q'):
            raise StopIteration


# def recv(host, port):
#     global connected
#     sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
#     response = '1 2 3 4 5'  # just to get message from train
#     sock.settimeout(0.2)
#     while 1:
#         sock.sendto(response.encode(), (host, port))  # trying to connect
#         try:
#             data, addr = sock.recvfrom(1024)  # connected!
#             data = data.decode('ascii').split()
#             if data[0] == '22':
#                 connected = True
#             print("received message: %s" % data)
#             time.sleep(0.05)
#         except TimeoutException:  # connection failed!
#             print('connecting...')
#             continue


def recv_bindip(host, port):
    sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    sock.bind((host, port))
    sock.settimeout(0.2)
    while 1:
        try:
            data, addr = sock.recvfrom(1024)
            data = data.decode('ascii').split()
            print(f'received message: {data}')
            time.sleep(0.05)
        except TimeoutException:
            print('connecting...')
            continue


def send(host, port, response):
    sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    sock.sendto(response.encode(), (host, port))


if __name__ == '__main__':
    time.sleep(20)
    connected = True
    # message = ''
    # recv_thread = Thread(target=recv)
    # recv_thread.start()
    detect()
